#include "linear_steady_state.h"
#include "vehicle_lateral_speed_yaw.h"
#include "steady_state_test_path.h"
using namespace cvx;
using namespace auto_ros::control;
int main()
{
	std::string yaml_file = steady_state_test_path + "vehicle_speed_yaw.yaml";
	std::shared_ptr<VehicleLateralSpeedYaw> car_ptr = std::make_shared<VehicleLateralSpeedYaw>(yaml_file);
	std::shared_ptr<ContinuousLinearSys> dyn_ptr = car_ptr;
	LinearSteadyState test_ss_state(dyn_ptr);
	double speed = 20;
	car_ptr->para_update_matrix(speed);
	Eigen::VectorXd ditur_vec(car_ptr->distur_dim());
	ditur_vec << 0.02;
	Eigen::VectorXd steady_state(car_ptr->state_dim());
	Eigen::VectorXd steady_ctr(car_ptr->control_dim());
	Eigen::VectorXd slack_vec(car_ptr->state_dim());
	test_ss_state.add_slack_states({3});
	test_ss_state.qp_.addConstraint(box(-10. / 180.0 * M_PI,
										(test_ss_state.x_(0) + par(car_ptr->lf()) * test_ss_state.x_(1)) / dynpar(speed) - test_ss_state.u_(0),
										10. / 180.0 * M_PI));
	test_ss_state.qp_.addConstraint(box(-10. / 180.0 * M_PI,
										(test_ss_state.x_(0) - par(car_ptr->lr()) * test_ss_state.x_(1)) / dynpar(speed),
										10. / 180.0 * M_PI));
	test_ss_state.init_solver_ptr();
	test_ss_state.calc_steady_state(ditur_vec, steady_state, steady_ctr, slack_vec);
	double alpha_f = (eval(test_ss_state.x_(0)) + car_ptr->lf() * eval(test_ss_state.x_(1))) / speed - eval(test_ss_state.u_(0));
	double alpha_r = (eval(test_ss_state.x_(0)) - car_ptr->lr() * eval(test_ss_state.x_(1))) / speed;
	std::cout << "steady state at " << speed << " is:" << steady_state << std::endl;
	std::cout << "steady ctr at " << speed << " is:" << steady_ctr << std::endl;
	std::cout << "slack vec  at " << speed << " is:" << slack_vec << std::endl;
	std::cout << "alpha f at " << speed << " is:" << alpha_f << std::endl;
	std::cout << "alpha r at " << speed << " is:" << alpha_r << std::endl;

	speed = 40;
	car_ptr->para_update_matrix(speed);
	test_ss_state.calc_steady_state(ditur_vec, steady_state, steady_ctr, slack_vec);
	alpha_f = (eval(test_ss_state.x_(0)) + car_ptr->lf() * eval(test_ss_state.x_(2))) / speed - eval(test_ss_state.u_(0));
	alpha_r = (eval(test_ss_state.x_(0)) - car_ptr->lr() * eval(test_ss_state.x_(2))) / speed;

	std::cout << "steady state at " << speed << " is:" << steady_state << std::endl;
	std::cout << "steady ctr at " << speed << " is:" << steady_ctr << std::endl;
	std::cout << "slack vec  at " << speed << " is:" << slack_vec << std::endl;
	std::cout << "alpha f at " << speed << " is:" << alpha_f << std::endl;
	std::cout << "alpha r at " << speed << " is:" << alpha_r << std::endl;
	/*calc_steady_state(const Eigen::VectorXd ditur_vec,
										  Eigen::VectorXd &steady_state,
										  Eigen::VectorXd &steady_ctr,
										  Eigen::VectorXd &slack_vec)	
	*/
}